#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include <libz800.h>

int main(int argc, char **argv)
{
	setvbuf(stdout, NULL , _IOLBF , 1024);
	setvbuf(stderr, NULL , _IOLBF , 1024);

	bool help = false;
	bool loop = false;
	bool reconnect = false;
	bool dumpProperties = false;
	bool verbose = false;
	bool dumpEEPROM = false;

	for(int i = 1; i < argc; ++i)
	{
		if(!strcmp(argv[i], "-h")) help = true;
		if(!strcmp(argv[i], "-v")) verbose = true;
		if(!strcmp(argv[i], "-l")) loop = true;
		if(!strcmp(argv[i], "-r")) reconnect = true;
		if(!strcmp(argv[i], "-e")) dumpEEPROM = true;
		if(!strcmp(argv[i], "-d")) dumpProperties = true;
	}

	if(help)
	{
		printf("Usage: %s [OPTION]...\n"
			   "\n"
			   "Options:\n"
			   "  -h        print this help, then exit\n"
			   "  -v        enable verbose libusb output\n"
			   "  -l        continusly display tracker data\n"
			   "  -r        after connection, disconnect and reconnect once\n"
			   "  -e        dump EEPROM\n"
			   "  -d        dump Properties\n",
#ifdef _DEBUG
			   "z800testD"
#else
			   "z800test"
#endif
				);
		return EXIT_SUCCESS;
	}

	z800_device *dev = z800_connect(verbose ? 7 : 2);
	if(dev == NULL)
	{
		fprintf(stderr, "z800_connect() failed\n");
		return EXIT_FAILURE;
	}

	z800_set_debug_level(dev, verbose ? 5 : 2);

	fflush(stdout);
	fflush(stderr);

	printf("*** Connected to Z800\n");	

	int firmware = z800_get_firmware_version(dev);
	if(firmware < 0)
	{
		fprintf(stderr, "z800_get_firmware_version() failed\n");
	}
	else
	{
		printf("*** Firmware version: %d.%d\n", firmware >> 8, firmware & 0xff);
	}

	if(dumpEEPROM)
	{
		if(z800_dump_eeprom(dev) == 0)
		{
			fprintf(stderr, "z800_dump_eeprom() failed\n");
		}
	}

	if(dumpProperties)
	{
		const char * const *p = z800_get_properties(dev);

		if(p == NULL)
		{
			fprintf(stderr, "z800_get_properties() failed\n");
		}
		else
		{
			for(; *p; ++p)
			{
				printf("*** Dumping next property\n"
					   "Name: %s\n", *p);

				struct z800_property info;
				if(z800_get_property_meta_info(dev, *p, &info) == 0)
				{
					fprintf(stderr, "z800_get_property_meta_info() failed\n");
					continue;
				}

				printf("Stereo: %d\n", info.stereo);
				printf("Min: %d\n", info.min_value);
				printf("Max: %d\n", info.max_value);
				printf("Default: %d\n", info.default_value);

				int v;
				if(z800_read_property(dev, *p, &v, Z800_FALSE) == 0)
				{
					fprintf(stderr, "z800_read_property() failed\n");
					continue;
				}

				if(info.stereo)
				{
					printf("Left value: %d\n", v);
					if(z800_read_property(dev, *p, &v, Z800_TRUE) == 0)
					{
						fprintf(stderr, "z800_read_property() failed\n");
						continue;
					}
					printf("Right value: %d\n", v);
				}
				else
				{
					printf("Value: %d\n", v);
				}
			}
		}
	}

	printf("*** Raw data from Z800:\n");

	for(;;)
	{		
		float gyr[3], acc[3], mag[3];
		z800_poll_tracker(dev);
		z800_get_raw_tracker_data(dev, &gyr, &acc, &mag);

		printf("\r*** G %+.3f %+.3f %+.3f   "
			 "A %+.3f %+.3f %+.3f   "
			 "M %+.3f %+.3f %+.3f",
			 gyr[0], gyr[1], gyr[2],
			 acc[0], acc[1], acc[2],
			 mag[0], mag[1], mag[2]);
		fflush(stdout);
		if(loop == false)
		{
			printf("\n");
			break;
		}
	}

	if(reconnect)
	{
		z800_set_debug_level(dev, verbose ? 7 : 2);
		z800_disconnect(dev);
		printf("*** Reconnecting...\n");
		dev = z800_connect(verbose ? 7 : 2);
		z800_disconnect(dev);
	}
}
